#ifndef PFC
#define PFC

#include "usart.h"
#include "hrtim.h"
#include "adc.h"
#include "gpio.h"
#include "tim.h"
#include "power_meas_sine_analyzer.h"
#include "SPLL_1ph_SOGI.h"
#include "PID.h"
#include "INA228.h"
#include "ADS1256.h"
#include "comKey.h"
#include "oled.h"
#include "Menu.h"

#define INVERTER_1
//#define INVERTER_2

#ifdef INVERTER_1
#define VGRID_OFFSET (2002.8622f)
#define VGRID_COEFF  (0.033333f)
#define IGRID_OFFSET (2003.3163f)
#define IGRID_COEFF  (0.003137f)
#define CURRENT_COMPENSATE_SIN (0.19611613f)
#define CURRENT_COMPENSATE_COS (0.98058067f)
#define IRMS_COEFF (1.4073f)
#define IRMS_BIAS (-0.0083f)

#define GRID_FREQ_HZ (51.3f)
#define PFC_ISR_FREQ_HZ (25000.0f)
#define VGRID_MAX (40.0f)
#define BUFFER_SIZE 125

#define VGRID_TIMEOUT 2

#define PFC_PI ((float32_t)3.141592653589)
#define PFC_DEFAULT_DUTY (0.45f)

#define PFC_GI_KP (0.05f)
#define PFC_GI_KI (0.83f)

#define PFC_GV_KP (0.01f)
#define PFC_GV_KI (0.18f)

#define PFC_GI_LIMIT (0.5f)
#define PFC_GV_LIMIT (0.5f)
#endif

#ifdef INVERTER_2
#define VGRID_OFFSET (1952.0f)
#define VGRID_COEFF  (0.03273f)
#define IGRID_OFFSET (2024.5f)
#define IGRID_COEFF  (0.0032f)
#define CURRENT_COMPENSATE_SIN (0.0f)
#define CURRENT_COMPENSATE_COS (1.0f)
#define IRMS_COEFF (1.4286f)
#define IRMS_BIAS (0.0163f)

#define GRID_FREQ_HZ (51.3f)
#define PFC_ISR_FREQ_HZ (25000.0f)
#define VGRID_MAX (40.0f)
#define BUFFER_SIZE 125

#define VGRID_TIMEOUT 2

#define PFC_PI ((float32_t)3.141592653589)
#define PFC_DEFAULT_DUTY (0.45f)

#define PFC_GI_KP (0.048f)
#define PFC_GI_KI (0.83f)

#define PFC_GV_KP (0.01f)
#define PFC_GV_KI (0.18f)

#define PFC_GI_LIMIT (0.5f)
#define PFC_GV_LIMIT (0.5f)
#endif



typedef enum Working_Mode_t{
  mode_VoltageSource,
  mode_CurrentSource
}Working_Mode_t;

typedef enum FSM_Status_t{
    status_WaitForGrid,
    status_WaitForBus,
    status_WaitForZero,
    status_NormalOperation,
    status_Fault
}FSM_Status_t;

typedef enum Fault_Flags_t{
    fault_gridOverVoltage,
    fault_gridUnderVoltage,
    fault_gridOverCurrent,
    fault_busOverVoltage,
    fault_busUnderVoltage,
    fault_PLLLost,
    fault_freqCollapse,
    fault_reversePower
}Fault_Flags_t;

typedef struct Delay_Buffer_t{
	float data[BUFFER_SIZE];
	uint16_t front, rear;
}Delay_Buffer_t;

typedef struct alphabeta_t{
	float alpha;
	float beta;
}alphabeta_t;

typedef struct qd_t{
	float q;
	float d;
}qd_t;

extern FSM_Status_t FSM_Status;
extern Working_Mode_t Working_Mode;
extern Fault_Flags_t PFC_Faults;

extern Delay_Buffer_t Grid_Vol_Buffer,Grid_Cur_Buffer;
extern SPLL_1PH_SOGI Grid_SPLL;
extern POWER_MEAS_SINE_ANALYZER Grid_Analyzer;

extern Pid_Controller_t GI_Id_Ctrl,GI_Iq_Ctrl,GV_VBus_Ctrl,GV_VGrid_Ctrl;

extern float VBus,Angle_Radians,Internal_Sine,Internal_Cosine,Last_VGrid,I_ref;

extern alphabeta_t VGrid_ab,IGrid_ab,Duty_ab;
extern qd_t VGrid_qd,IGrid_qd,Duty_qd;

extern float IGrid_q_ref,IGrid_d_ref,VBus_ref,VGrid_ref; 
extern volatile float duty;

extern uint16_t ADC_RawData[4];

//function prototypes
static inline void PFC_FSM_Update();
static inline void PFC_ISR1();
static inline void PFC_ISR2();
static inline void Generate_Internal_Reference();
static inline void Read_Sensor_Data_HS();
static inline void Read_Sensor_Data_LS();
static inline void Run_Current_Loop();
static inline void Run_Voltage_Loop();
static inline void Run_Transform();
static inline void Buffer_Insert(float data,Delay_Buffer_t* p);
static inline float Buffer_Get_Front(Delay_Buffer_t* p);
static inline void Clamp(float* data,float upper,float lower);
static inline void Park(qd_t* qd,alphabeta_t alphabeta,float cosine,float sine);
static inline void Inv_Park(alphabeta_t* alphabeta,qd_t qd,float cosine,float sine);
static inline float Slew_Func(float *slewVal, float refVal, float slewRate);
static inline void Gate_Enable();
static inline void Gate_Disable();
static inline void Relay_Close();
static inline void Relay_Open();
static inline void Set_PWM_Duty(float duty);
void PFC_Init();
void Buffer_Init(Delay_Buffer_t* p);
void PFC_FSM_Update();
void PFC_Peripheral_Setup();
void PFC_While();

//function declaration
static inline void PFC_FSM_Update()
{
  static int Wait_Time = 0;
    switch(FSM_Status){
        case status_WaitForGrid:
            #ifdef INVERTER_1
            Wait_Time++;
            if(Wait_Time > VGRID_TIMEOUT*PFC_ISR_FREQ_HZ)
            {
              Working_Mode = mode_VoltageSource;
              Gate_Enable();
              Relay_Close();
              FSM_Status = status_NormalOperation;
            }
            #endif  
            if(Wait_Time>VGRID_TIMEOUT*PFC_ISR_FREQ_HZ/2 && Grid_Analyzer.vRms>22 && Grid_SPLL.u_Q[0]<0.05)
                FSM_Status = status_WaitForBus;
            break;
        case status_WaitForBus:
            if(VBus > 38)
            {
                HAL_GPIO_WritePin(LD2_GPIO_Port,LD2_Pin,GPIO_PIN_SET);
                FSM_Status = status_WaitForZero;
            }
            break;
        case status_WaitForZero:
            if(Last_VGrid < 0 && VGrid_ab.alpha >=0)
            {
                Gate_Enable();
                Relay_Close();
                FSM_Status = status_NormalOperation;
            }
            break;    
        case status_NormalOperation:
            if(Working_Mode == mode_CurrentSource)
            {
                if(Grid_SPLL.u_Q[0] > 0.05)
                {
                    PFC_Faults = fault_PLLLost;
                    FSM_Status = status_Fault;
                }
                if(Grid_Analyzer.vRms < 5)
                {
                    PFC_Faults = fault_gridUnderVoltage;
                    FSM_Status = status_Fault;
                }
                if(VBus < 38)
                {
                    PFC_Faults = fault_busUnderVoltage;
                    FSM_Status = status_Fault;
                }
                if(Grid_SPLL.fo < 45 || Grid_SPLL.fo > 55)
                {
                    PFC_Faults = fault_freqCollapse;
                    FSM_Status = status_Fault;
                }
                if(IGrid_ab.alpha > 5)
                {
                    PFC_Faults = fault_gridOverCurrent;
                    FSM_Status = status_Fault;
                }
            }
            if(VBus > 65)
            {
                PFC_Faults = fault_busOverVoltage;
                FSM_Status = status_Fault;
            }
            break;
        case status_Fault:
            Gate_Disable();
            Relay_Open();
            break;            
    }
}

static inline void PFC_ISR1()//25000Hz
{
  Read_Sensor_Data_HS();
  Run_Transform();
  if(Working_Mode == mode_VoltageSource)
    Generate_Internal_Reference();
  
  PFC_FSM_Update();
 
  Grid_Analyzer.v = VGrid_ab.alpha;
  Grid_Analyzer.i = IGrid_ab.alpha;
  POWER_MEAS_SINE_ANALYZER_run(&Grid_Analyzer);
  SPLL_1PH_SOGI_run(&Grid_SPLL,VGrid_ab.alpha/VGRID_MAX);

  if(Working_Mode == mode_CurrentSource && FSM_Status == status_NormalOperation)
  {
    IGrid_q_ref = (I_ref*IRMS_COEFF+IRMS_BIAS)*CURRENT_COMPENSATE_COS;
    IGrid_d_ref = (I_ref*IRMS_COEFF+IRMS_BIAS)*CURRENT_COMPENSATE_SIN;
    Run_Current_Loop();
  }

   if(Working_Mode == mode_CurrentSource)
     Inv_Park(&Duty_ab,Duty_qd,Grid_SPLL.cosine,Grid_SPLL.sine);
   else  
    Inv_Park(&Duty_ab,Duty_qd,Internal_Cosine,Internal_Sine);

  duty = Duty_ab.alpha + 0.5;
  Clamp(&duty, 0.98, 0.02);
  Set_PWM_Duty(duty);
}

static inline void PFC_ISR2()//200Hz
{
  Read_Sensor_Data_LS();

  if(Working_Mode == mode_VoltageSource)
    Run_Voltage_Loop();
}

static inline void Generate_Internal_Reference()
{
  static float step = (1 / PFC_ISR_FREQ_HZ) * GRID_FREQ_HZ;
  static float theta = 0;

  theta += step;
  if (theta > 1)
    theta -= 1;

  Angle_Radians = theta * PFC_PI * 2.0f;

  Internal_Sine = sinf(Angle_Radians);
  Internal_Cosine = cosf(Angle_Radians);
}

static inline void Read_Sensor_Data_HS()
{  
  Last_VGrid = VGrid_ab.alpha;

  #ifdef INVERTER_2
  VGrid_ab.alpha = -((float)ADC_RawData[0]-VGRID_OFFSET)*VGRID_COEFF;
  #endif
  #ifdef INVERTER_1
  VGrid_ab.alpha = ((float)ADC_RawData[0]-VGRID_OFFSET)*VGRID_COEFF;
  #endif
  VGrid_ab.beta = Buffer_Get_Front(&Grid_Vol_Buffer);
  Buffer_Insert(VGrid_ab.alpha, &Grid_Vol_Buffer);
  

  IGrid_ab.alpha = ((float)ADC_RawData[1]-IGRID_OFFSET)*IGRID_COEFF;
  IGrid_ab.beta = Buffer_Get_Front(&Grid_Cur_Buffer);
  Buffer_Insert(IGrid_ab.alpha, &Grid_Cur_Buffer);

}

static inline void Read_Sensor_Data_LS()
{
  VBus = 0.975*VBus + 0.025*INA228_Get_VBUS();   
}

static inline void Run_Current_Loop()
{
  Duty_qd.q = Pid_Cal(&GI_Iq_Ctrl,IGrid_q_ref,IGrid_qd.q);
  Duty_qd.d = Pid_Cal(&GI_Id_Ctrl,IGrid_d_ref,IGrid_qd.d);

  Duty_qd.q -= 0.5*VGrid_qd.q/VBus;
  Duty_qd.d -= 0.5*VGrid_qd.d/VBus;

  Clamp(&Duty_qd.q,0.5,-0.5);
  Clamp(&Duty_qd.d,0.5,-0.5);
}

static inline void Run_Voltage_Loop()
{
  Duty_qd.q = -Pid_Cal(&GV_VGrid_Ctrl,VGrid_ref,Grid_Analyzer.vRms);
  Duty_qd.d = 0;

  Clamp(&Duty_qd.q,0,-0.5);
  Clamp(&Duty_qd.d,0,-0.5);
}

static inline void Run_Transform()
{
  if(Working_Mode == mode_CurrentSource)
  {
    Park(&VGrid_qd,VGrid_ab,Grid_SPLL.cosine,Grid_SPLL.sine);
    Park(&IGrid_qd,IGrid_ab,Grid_SPLL.cosine,Grid_SPLL.sine);
  }
  else
  {
    Park(&VGrid_qd,VGrid_ab,Internal_Cosine,Internal_Sine);
    Park(&IGrid_qd,IGrid_ab,Internal_Cosine,Internal_Sine);
 }  
}

static inline void Buffer_Insert(float data,Delay_Buffer_t* p)
{
	if ((p->rear + 1) % BUFFER_SIZE == p->front)
	{
		p->data[p->front] = 0;
		p->data[p->rear] = data;
		p->front = (p->front + 1) % BUFFER_SIZE;
		p->rear = (p->rear + 1) % BUFFER_SIZE;
	}
	else
	{
		p->data[p->rear] = data;
		p->rear = (p->rear + 1) % BUFFER_SIZE;
	}
}

static inline float Buffer_Get_Front(Delay_Buffer_t* p)
{
	return p->data[p->front];
}

static inline void Clamp(float* data,float upper,float lower)
{
	if(*data>upper) 
		*data = upper;
	else if(*data<lower)
		*data = lower;
}

static inline void Park(qd_t* qd,alphabeta_t alphabeta,float cosine,float sine)
{
	qd->d = cosine*alphabeta.alpha + sine*alphabeta.beta;
	qd->q = -sine*alphabeta.alpha + cosine*alphabeta.beta;
}

static inline void Inv_Park(alphabeta_t* alphabeta,qd_t qd,float cosine,float sine)
{
	alphabeta->alpha = cosine*qd.d - sine*qd.q;
	alphabeta->beta = sine*qd.d - cosine*qd.q;
}

static inline float Slew_Func(float *slewVal, float refVal, float slewRate)
{
  static float diff = 0;
  diff = refVal - *slewVal;
  if (diff >= slewRate)
  {
    *slewVal += slewRate;
    return (1);
  } else if (-diff >= slewRate)
  {
    *slewVal -= slewRate;
    return (-1);
  } else
  {
    *slewVal = refVal;
    return (0);
  }
}

static inline void Gate_Enable()
{
	HAL_HRTIM_WaveformOutputStart(&hhrtim1,HRTIM_OUTPUT_TA1|HRTIM_OUTPUT_TA2|HRTIM_OUTPUT_TB1|HRTIM_OUTPUT_TB2);
}

static inline void Gate_Disable()
{
	HAL_HRTIM_WaveformOutputStop(&hhrtim1,HRTIM_OUTPUT_TA1|HRTIM_OUTPUT_TA2|HRTIM_OUTPUT_TB1|HRTIM_OUTPUT_TB2);
}

static inline void Relay_Close()
{
  HAL_GPIO_WritePin(RELAY_2_GPIO_Port,RELAY_2_Pin,GPIO_PIN_RESET);
}

static inline void Relay_Open()
{
  HAL_GPIO_WritePin(RELAY_2_GPIO_Port,RELAY_2_Pin,GPIO_PIN_SET);
}

static inline void Set_PWM_Duty(float duty)
{
  HRTIM1->sMasterRegs.MCMP1R = (uint16_t)(duty*HRTIM1->sMasterRegs.MPER);
  HRTIM1->sMasterRegs.MCMP2R = HRTIM1->sMasterRegs.MCMP1R/2;
}

#endif